#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "tnt_126/tnt_array2d.h"
#include "tnt_126/tnt_array1d.h"
#include "jama125/jama_lu.h"

#define Quadric


extern void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);

void vcross(double v[3], double w[3], double a[3]) 
{
   a[0] = v[1]*w[2] - v[2]*w[1];
   a[1] = v[2]*w[0] - v[0]*w[2];
   a[2] = v[0]*w[1] - v[1]*w[0];
}

int move_point(float vin[3], int np, float points[][3], float new_pos[3], float normal[3], int correlation,float *surface) 
{
  // Matrix a = Matrix(6, 6);
 //  Matrix ainv, product;

   double cgx = 0., cgy = 0., cgz = 0., dx, dy, dz, A[3][3], V[3][3], eigenvalues[3];
   int  ii, i, j, k, n, debug = 0;
   double radius, rsq, rr, sizec, sum, fac, x, y, v, x2, y2, x3, y3,
      x4, y4,x5=0,y5=0, sx=0, sy=0, sx2=0, sxy=0, sy2=0, sx3=0, sx2y=0, sxy2=0, sy3=0, sx4=0, sx3y=0,
      sx2y2=0, sxy3=0, sy4=0, sv=0, sxv=0, syv=0,sx3y2=0,sx2y3=0,sx4y=0,sxy4=0, sx5=0,sy5=0,sx4y2=0,sx3y3=0,sx2v=0,sx5y=0,sx2y4=0,sxy5=0,sx6=0,sy6=0, sxyv=0,sx2yv=0,sxy2v=0,sx3v=0,sy3v=0, sy2v=0,
	  #ifdef Quadric
b[6],e[6],
	  #else
	  b[10], e[10], 
#endif
	  w = 1., sw,
      axis_min, u, esum, maxesum = 0, temp, dmax, rmaxsq=0, vv[3], ww[3],
      tx, ty, dt;

   
		TNT::Array1D<double> eres(10);

   for (i = 0; i < 3; ++i) 
   {
		cgx += points[i][0];
		cgy += points[i][1];
		cgz += points[i][2];
   }
   
   cgx /= 3.0;
   cgy /= 3.0;
   cgz /= 3.0;
   
   if(debug)
      printf("center of gravity %f %f %f\n", cgx, cgy, cgz);

   if (correlation) 
   {
	   for(i = 0; i < 3; ++i)
		   for(j = 0; j < 3; ++j)
			   A[i][j] = 0;

	   for (i = 0; i < np; ++i) 
	   {
		   dx = points[i][0] - cgx;
		   dy = points[i][1] - cgy;
		   dz = points[i][2] - cgz;
	 
		   rr = dx*dx + dy*dy + dz*dz;
	 
		   if (rr > rmaxsq) rmaxsq = rr;
         
		   if (correlation == 2) 
		   {
			   A[0][0] += dy*dy + dz*dz;
			   A[1][1] += dx*dx + dz*dz;
			   A[2][2] += dx*dx + dy*dy;
			   A[0][1] -= dx*dy;
			   A[0][2] -= dx*dz;
			   A[1][2] -= dy*dz;
		   }
		   else 
		   {                     
			   // correlation            
			   A[0][0] += dx*dx;            
			   A[1][1] += dy*dy;            
			   A[2][2] += dz*dz;            
			   A[0][1] += dx*dy;            
			   A[0][2] += dx*dz;            
			   A[1][2] += dy*dz;
		   }
	   }
	   A[1][0] = A[0][1];
	   A[2][0] = A[0][2];
	   A[2][1] = A[1][2];

	   eigen_decomposition(A, V, eigenvalues);

	   if (debug)
		   printf("eigenvalues %f %f %f\n", eigenvalues[0], eigenvalues[1], eigenvalues[2]);
     
	   if(debug) 
	   {
		   printf("original V matrix\n");
		   for (i = 0; i < 3; ++i)
           printf("%22.18f %22.18f %22.18f\n", V[i][0], V[i][1], V[i][2]);
       }

	   if(correlation == 1) 
		   for (i = 0; i < 3; ++i) 
		   {            
			   temp = V[i][0];            
			   V[i][0] = V[i][2];
			   V[i][2] = temp;
		   }

		   if(debug) 
		   {
			   printf("permuted V matrix\n");
			   for (i = 0; i < 3; ++i)
				   printf("%f %f %f\n", V[i][0], V[i][1], V[i][2]);
		   }
   }
   else 
   {
	   for (i = 0; i < 3; ++i) 
	   {
		   vv[i] = points[1][i] - points[0][i];
		   ww[i] = points[2][i] - points[0][i];
	   }
      
	   vcross(vv, ww, V[2]);
	   vv[0] = vv[1] = vv[2] = 0;
	   axis_min = 1000000000.;
      
	   for(i = 0; i < 3; ++i)    
	   {
		   if(V[2][i] < axis_min) 
		   {
			   axis_min = V[2][i];
			   j = i;
		   }
	   }
      

	   vv[j] = 1.;
	   vcross(V[2], vv, V[1]);
	   vcross(V[1], V[2], V[0]);
      
	   for (i = 0; i < np; ++i) 
	   {
		   dx = points[i][0] - cgx;
		   dy = points[i][1] - cgy;
		   dz = points[i][2] - cgz;

		   rr = dx*dx + dy*dy + dz*dz;
		   if (rr > rmaxsq) 			  
			   rmaxsq = rr;	 
	   }
	}
   
	for(j = 0; j < 3; ++j) 
	{
		sum = 0;
		for (i = 0; i < 3; ++i)
			sum += V[i][j]*V[i][j];
		fac = sqrt(sum);

		for (i = 0; i < 3; ++i)
			V[i][j] /= fac;
	}
   
	if(debug) 
	{
		printf("V matrix\n");
      
		for (i = 0; i < 3; ++i)
			printf("%f %f %f\n", V[i][0], V[i][1], V[i][2]);
	}

	rsq = rmaxsq*2.;
	n = 0;
	sx = sy = sx2 = sxy = sy2 = sx3 = sx2y = sxy2 = sy3 = sx4 = sx3y = sx2y2 = sxy3 = sy4 = sxv = syv = sv = sx2v = sxyv = sy2v = sw = 0.;

	for (i = 0; i < np; ++i) 
	{
		x = V[0][0]*(points[i][0]-cgx) + V[1][0]*(points[i][1]-cgy) + V[2][0]*(points[i][2]-cgz);
		y = V[0][1]*(points[i][0]-cgx) + V[1][1]*(points[i][1]-cgy) + V[2][1]*(points[i][2]-cgz);
		v = V[0][2]*(points[i][0]-cgx) + V[1][2]*(points[i][1]-cgy) + V[2][2]*(points[i][2]-cgz);
		x2 = x*x;
		x3 = x2*x;
		x4 = x3*x;
		x5=x4*x;
		y2 = y*y;
		y3 = y2*y;
		y4 = y3*y;
		y5=y4*y;
		w = 1. - ((points[i][0]-cgx)*(points[i][0]-cgx) + (points[i][1]-cgy)*(points[i][1]-cgy) +(points[i][2]-cgz)*(points[i][2]-cgz))/rsq;

		++n;
		sw += w;
		sx += x*w;
		sy += y*w;
		sx2 += x2*w;
		sxy += x*y*w;
		sy2 += y2*w;
		sx3 += x3*w;
		sx2y += x2*y*w;
		sxy2 += x*y2*w;
		sy3 += y3*w;
		sx4 += x4*w;
		sx3y += x3*y*w;
		sx2y2 += x2*y2*w;
		sxy3 += x*y3*w;
		sy4 += y4*w;


		sx3y2+=x3*y2*w;
		sx2y3+=x2*y3*w;
		sx4y+=x4*y*w;

		sx5+=x4*x*w;
		sy5+=y4*y*w;
		sx4y2+=x4*y2*w;
		sx3y3+=x3*y3*w;

		sx5y+=x5*y*w;
		sx2y4=x2*y4*w;
		sxy5=x*y5*w;
		sx6+=x5*x*w;
		sy6+=y5*y*w;



		sxv += x*v*w;
		syv += y*v*w;
		sv += v*w;
		sx2v += x2*v*w;
		sxyv += x*y*v*w;
		sy2v += y2*v*w;
		sx2yv+=x2*y*v*w;
		sxy2v+=x*y2*v*w;

		sx3v+=x2*x*v*w;

		sy3v+=y3*v*w;
     }
   
	if(n > 5) 
	{

#ifdef Quadric


		      b[0] = sxv;
      b[1] = syv;
      b[2] = sv;
      b[3] = sx2v;
      b[4] = sxyv;
      b[5] = sy2v;

	  		TNT::Array2D<double> a(6,6);

      a[0][ 0] = sx2;
      a[0][1] = sxy;
      a[0][2] = sx;
      a[0][3] = sx3;
      a[0][4] = sx2y;
      a[0][5] = sxy2;
      a[1][1] = sy2;
      a[1][2] = sy;
      a[1][3] = sx2y;
      a[1][4] = sxy2;
      a[1][5] = sy3;
      a[2][2] = n;
      a[2][3] = sx2;
      a[2][4] = sxy;
      a[2][5] = sy2;
      a[3][3] = sx4;
      a[3][4] = sx3y;
      a[3][5] = sx2y2;
      a[4][4] = sx2y2;
      a[4][5] = sxy3;
      a[5][5] = sy4;

      for (i = 1; i < 6; ++i)
         for (j = 0; j < i; ++j)
            a[i][j] = a[j][ i];

		JAMA::LU<double> lu(a);


		TNT::Array1D<double> bin(6,b);

		eres=lu.solve(bin);


#else
		b[0] = sv;
		b[1] = sxv;
		b[2] = syv;
		b[3] = sxyv;
		b[4] = sx2v;
		b[5] = sy2v;
		b[6]=sx2yv;
		b[7]=sxy2v;
		b[8]=sx3v;
		b[9]=sy3v;

		TNT::Array2D<double> a(10,10);





		a[0][0]=n;
		a[0][1]=sx;
		a[0][2]=sy;
		a[0][3]=sxy;
		a[0][4]=sx2;
		a[0][5]=sy2;
		a[0][6]=sx2y;
		a[0][7]=sxy2;
		a[0][8]=sx3;
		a[0][9]=sy3;

		a[1][1]=sx2;
		a[1][2]=sxy;
		a[1][3]=sx2y;
		a[1][4]=sx3;
		a[1][5]=sxy2;
		a[1][6]=sx3y;
		a[1][7]=sx2y2;
		a[1][8]=sx4;
		a[1][9]=sxy3;

		a[2][2]=sy2;
		a[2][3]=sxy2;
		a[2][4]=sx2y;
		a[2][5]=sy3;
		a[2][6]=sx2y2;
		a[2][7]=sxy3;
		a[2][8]=sx3y;
		a[2][9]=sy4;
		

		a[3][3]=sx2y2;
		a[3][4]=sx3y;
		a[3][5]=sxy3;
		a[3][6]=sx3y2;
		a[3][7]=sx2y3;
		a[3][8]=sx4y;
		a[3][9]=sxy4;

		a[4][4]=sx4;
		a[4][5]=sx2y2;
		a[4][6]=sx4y;
		a[4][7]=sx3y2;
		a[4][8]=sx5;
		a[4][9]=sx2y3;

		a[5][5]=sy4;
		a[5][6]=sx2y3;
		a[5][7]=sxy4;
		a[5][8]=sx3y2;
		a[5][9]=sy5;

		a[6][6]=sx4y2;
		a[6][7]=sx3y3;
		a[6][8]=sx5y;
		a[6][9]=sx2y4;

		a[7][7]=sx2y4;
		a[7][8]=sx4y2;
		a[7][9]=sxy5;

		a[8][8]=sx6;
		a[8][9]=sx3y3;

		a[9][9]=sy6;

		


		for (i = 1; i < 10; ++i)
			for (j = 0; j < i; ++j)
            a[i][ j] = a[j][ i];




		JAMA::LU<double> lu(a);


		TNT::Array1D<double> bin(10,b);

		eres=lu.solve(bin);

#endif

	}	
	else 
	{
		printf("only %d points input for call to move_point\n",np);
		getchar();
		return 0; 
	}


   
	x = V[0][0]*(vin[0]-cgx) + V[1][0]*(vin[1]-cgy) + V[2][0]*(vin[2]-cgz);
	y = V[0][1]*(vin[0]-cgx) + V[1][1]*(vin[1]-cgy) + V[2][1]*(vin[2]-cgz);


#ifdef Quadric

	   v = eres[0]*x + eres[1]*y + eres[2]+ eres[3]*x*x + eres[4]*x*y + eres[5]*y*y;
   tx = eres[0] + 2.*x*eres[3] + y*eres[4];
   ty = eres[1] + x*eres[4] + 2.*y*eres[5];

#else
	v = eres[0] + eres[1]*x + eres[2]*y+ eres[3]*x*y + eres[4]*x*x + eres[5]*y*y+eres[6]*x*x*y+eres[7]*x*y*y+eres[8]*x*x*x+eres[9]*y*y*y;
	tx =  eres[1] + eres[3]*y + eres[4]*2 +eres[6]*2*y+eres[7]*y*y+eres[8]*3;
	ty =  eres[2]+ eres[3]*x  + eres[5]*2+eres[6]*x*x+eres[7]*x*2+eres[9]*3;
#endif


	dt = 1./sqrt(1. + tx*tx + ty*ty);

	normal[0] = -tx*dt;
	normal[1] = -ty*dt;
	normal[2] = dt;
   
	if(debug) 
		for (i = 0; i < 6; ++i)
			printf("e[%d] = %f\n", i, eres[i]);
   
	new_pos[0] = cgx;
	new_pos[1] = cgy;
	new_pos[2] = cgz;

	for(k = 0; k < 3; ++k)
		new_pos[k] += x*V[k][0] + y*V[k][1] + v*V[k][2];
   
	float oldNormal[3];
	oldNormal[0]=normal[0];
	oldNormal[1]=normal[1];   
	oldNormal[2]=normal[2];

	for(k=0;k<3;++k)	 
		normal[k]=oldNormal[0]*V[k][0]+oldNormal[1]*V[k][1]+oldNormal[2]*V[k][2];

	int scount=0;

	if(surface)
	{
		for(int kx=0;kx<10;++kx)
			for(int ky=0;ky<10;++ky)
			{

				float sx=kx*0.1f-0.5f;
				float sy=ky*0.1f-0.5f;

#ifdef Quadric
				float sz = eres[0]*sx + eres[1]*sy + eres[2]+ eres[3]*sx*sx + eres[4]*sx*sy + eres[5]*sy*sy;
#else	
				float sz=eres[0] + eres[1]*sx + eres[2]*sy+ eres[3]*sx*sy + eres[4]*sx*sx + eres[5]*sy*sy+eres[6]*sx*sx*sy+eres[7]*sx*sy*sy+eres[8]*sx*sx*sx+eres[9]*sy*sy*sy;
#endif
				float newspos[3];

				newspos[0]=cgx;
				newspos[1]=cgy;
				newspos[2]=cgz;

				for(int ek = 0; ek < 3; ++ek)
					newspos[ek] += sx*V[ek][0] + sy*V[ek][1] + sz*V[ek][2];

				surface[scount*3]=newspos[0];
				surface[scount*3+1]=newspos[1];
				surface[scount*3+2]=newspos[2];

				//printf("[%f,%f,%f]\n",surface[scount*3],surface[scount*3+1],surface[scount*3+2]);
				scount++;
			}
	}
	return 1;   
}

/*
int main() {
   int i, j, np = 12;
   float points[13][3] = {
      {-1., -1., 0.},
      {1., 0., 0.},
      {0., 1., 0.},
      {1., 2., 0.},
      {-1., -2., 0.},
      {-2., 0., 0.},
      {2., -1.,0.},
      {-1., 2., 0.},
      {3., 1., 0.},
      {0., 4., 0.},
      {3., 4., 0.},
      {-3., 2., 0.},
      {0., 0., 0.} };
   double e[6] = {.006, .005, .04, .003, .002, .001}, x, y, z, ang = .0, sang, cang;
   float vin[3] = {0., 0., 0.}, new_pos[3], normal[3];

   cang = cos(ang);
   sang = sin(ang);
   for (i = 0; i < 13; ++i) {
      x = points[i][0];
      y = points[i][1];
      z = e[0]*x + e[1]*y + e[2]+ e[3]*x*x + e[4]*x*y + e[5]*y*y; //  + .001*x*x*x;
      points[i][0] = cang*x - sang*z;
      points[i][2] = sang*z + cang*z;
      if (i < -1)
         points[i][2] = e[0]*x + e[1]*y + e[2]+ e[3]*x*x + e[4]*x*y + e[5]*y*y; //  + .001*x*x*x;
      printf("point[%d] = {%f %f %f}\n", i, points[i][0], points[i][1], points[i][2]);
      }
   move_point(vin, 12, points, new_pos, normal, 0);
   for (i = 0; i < 3; ++i)
      printf("%d %f %f %f\n", i, new_pos[i], points[12][i], normal[i]);
   move_point(vin, 12, points, new_pos, normal, 1);
   for (i = 0; i < 3; ++i)
      printf("%d %f %f %f\n", i, new_pos[i], points[12][i], normal[i]);
   }
*/